/* PIN CONNECTIONS 
	Attopilot	
		Attopilot V : A0
		Attopilot A : A1

	Wheel
		Wheel sensor: D2
*/

//AN0
//const int VOLTAGE_PIN = 0;
//AN3
//const int TEMPERATURE_PIN = 3;
//AN4
//const int IREFA_PIN = 1;

//D2
const int WHEEL_PIN = 2;

//D3
//const int RELAY_PIN = 3;
const int MAX_TEMP_DEGREES = 70;

//GLOBAL VARIABLES
	//ATTOPILOT
	int VRaw; //This will store raw V ADC data
	int IRawA; //This will store raw I ADC data A
	int IRawB; //This will store raw I ADC data B
	float VFinal; //This will store the converted V data
	float IFinal; //This will store the converted I data
	
	float tempC; //This will store the degrees Celcius
	float tempAr[5];
	int arrayPosition = 0;

	bool _relaySet = false;

	//SPEED
	float _speed;
	unsigned long last_interrupt_time = 0;
	unsigned long last_button_interrupt_time = 0;
	unsigned long last_button_debounce_time = 0;
	unsigned long last_wheel_contact = 0;
	bool pressed;
	int wheelContact = 0; 

void setup()
{
	//Set wheel sensor pin to input
	pinMode(WHEEL_PIN, INPUT);
	//pinMode(RELAY_PIN, OUTPUT);
	
	//Start RS232
	//115200 is baud rate
	Serial.begin(115200);
}

void loop()
{
	/*if(!_relaySet)
	{
		digitalWrite(RELAY_PIN, HIGH);
	}*/

	wheelContact = digitalRead(WHEEL_PIN);
	
	unsigned long current_interrupt_time = millis();
	unsigned long wheel_time = millis();

	//Wheel contact and no bounce?
	if(wheelContact == HIGH && !pressed)
	{
		if(current_interrupt_time - last_button_debounce_time > 100)
		{
			pressed = true;
			//Time between interrupts
			unsigned long between = current_interrupt_time - last_button_interrupt_time;
		
			Serial.print("W,");
			Serial.print(between);
			Serial.println(",");

			last_button_interrupt_time = current_interrupt_time;
			last_button_debounce_time = current_interrupt_time;
			last_wheel_contact = current_interrupt_time;
		}
	}

	//No contact
	else if(wheelContact == LOW)
	{
		pressed = false;
		if(wheel_time - last_wheel_contact > 5000)
		{
			Serial.print("W,");
			Serial.print(-1);
			Serial.println(",");
			last_wheel_contact = wheel_time;
		}
	}

	//Sends sensor data at a rate of 4Hz to the on board PC
	if(current_interrupt_time - last_interrupt_time > 250)
	{	
		//Measurement
		//VRaw = analogRead(VOLTAGE_PIN);
		//IRawA = analogRead(IREFA_PIN);

		//Conversion according to attopilot board type
		//VFinal = VRaw/49.44; //45 Amp board
		//VFinal = VRaw/12.99; //90 Amp board
		//VFinal = VRaw/12.99; //180 Amp board  
  
		//IFinal = IRaw/14.9; //45 Amp board
		//IFinal = IRaw/7.4; //90 Amp board
		//IFinal = IRaw/3.7; //180 Amp board

		//Manual amperage using two parallel resistors of 0,1 ohm
		//IFinal = IRawA - IRawB / 0,05;

		//OVERRIDE - REMOVE
		VFinal = 0;
		IFinal = 0;
/*
		float CRaw = analogRead(TEMPERATURE_PIN);*/
		//Convert raw temp data to Celcius
		tempC = 0;
		/*tempAr[arrayPosition] = (5.0 * CRaw * 100.0)/1024.0;
		tempC = 0;

		for (int i = 0; i < 5; i++)
		{
			tempC += tempAr[i];
		}

		tempC /= 5;

		if(arrayPosition <= 4)
		{
			arrayPosition++;
		}

		else
		{
			arrayPosition = 0;
		}*/

		//Print waardes volgens communicatie opmaak
		//S, <VOLTAGE WAARDE>, <AMPERAGE WAARDE>
		Serial.print("S,");
		Serial.print(VFinal);
		Serial.print(",");
		Serial.print(IFinal);
		Serial.print(",");
		Serial.print(_speed);
		Serial.print(",");
		Serial.print(tempC);
		Serial.println(",");++++++

		/*if(tempC > MAX_TEMP_DEGREES)
		{
			digitalWrite(RELAY_PIN, LOW);
		}*/

		//Set last time to current time
		last_interrupt_time = current_interrupt_time;
	}





} 